#include "LnuCar.h"
/* LnuCar 省赛初赛版本
 * code by Asnull.
 * Date:2023/11/09
*/
int main(void)
{
	delay_init();//延时初始化
	OLED_Init(); // 屏幕初始化
	OLED_ShowString(1,1,"    Lnu-car");
	OLED_ShowString(3,1,"   Loading...");

	MPU6050_Init(); // 陀螺仪初始化
	delay_ms(50);//初始化延时
	TIM4_Int_Init(199,7199);//10Khz的计数频率，计数到200为20ms 

	LED_Init(); // LED灯初始化
	Motor_Init(); // 电机初始化
	IR_Init(); // 循迹传感器初始化
	Servo_Init(); // 舵机初始化
	Serial_Init(); // openmv串口通信
	HC05_Serial_Init(); // 蓝牙模块串口通信初始化
	AD_Init(); // ADC电压测量通道初始化
	
	OLED_Clear(); // 清屏
	OLED_ShowString(2,1,"Qxd:");  // 陀螺仪倾斜度
	OLED_ShowString(3, 1, "Vol:0.00V"); // 电池电压
	Servo1_SetAngle(15); // 舵机回到初始位置
	while (1)
	{
		// 开启主循迹
		track(mainSpeed,mainSpeed);

		Servo1_SetAngle(15); // 舵机回到初始位置

		// 小车状态检测
		checkCarStatus(10.0,5.0);

		// 显示陀螺仪数值
		sprintf((char *)tmp_buf,"%.2f ",Angle_Y_Final);
		OLED_ShowString(2,5,tmp_buf); 

		// 电池电压值
		Voltage = ((float)AD_GetValue() / 4095 * 3.300) * 8.360; 
		VoltageAverCal(&Voltage,&VoltageAver); // 计算平均值
		sprintf((char *)tmp_buf,"%.2fV ",VoltageAver);
		OLED_ShowString(3,5,tmp_buf);	
		
		if (!irState[0] && !irState[1] && !irState[2] && !irState[3])
		{  // 首次识别到黑线
			readCross(); // 识别黑色十字
			// 根据识别到的十字黑线的个数执行不同代码
			switch (count)
			{
				case 1:
					// 上桥
					GPIO_ResetBits(GPIOB, GPIO_Pin_12);  // 开启第一个指示灯
					goBridge();
					#if(isWork_OLED == 1)
						// 用于在屏幕中调试，在第三行第二个位置打印1则说明已完成上桥，下同
						OLED_ShowNum(4,3,1,1);
					#endif
					break;
				case 2:
					// 过台阶
					GPIO_ResetBits(GPIOB, GPIO_Pin_13);
					goStep();
					#if(isWork_OLED == 1)
						OLED_ShowNum(4,3,2,1);
					#endif
					break;
				case 3:	
					checkColorCard(); // 颜色卡片识别
					GPIO_ResetBits(GPIOB, GPIO_Pin_14);
					goTerminus(); // 冲向终点
					// while (1);
				default:
					break;
			}
		}
		
	}
}

// 平均电压计算
/*
 @ Voltage:瞬时电压值
 @ VoltageAver: 平均值
*/
void VoltageAverCal(float *Voltage,float *VoltageAver){
	static float VolSum;
	static unsigned int VolCount;
	if (VolCount < 50)
	{
		VolSum += *Voltage;
		VolCount++;
	}else
	{
		*VoltageAver = VolSum / VolCount;
		// 清零
		VolSum = 0;
		VolCount = 0;
	}
}

// 检测小车俯仰状态
/*
 @ uphillUperLimit:转为上波状态界限
 @ flatUperLimit : 平地上限
*/
short int checkCarStatus(float uphillUperLimit,float flatUperLimit){
	static float AngleSum;
	static unsigned int readAngleCount;
	static short int carStatus;// 小车状态 1 上坡，-1下坡 0平地
	unsigned int countTiem = 2; // 计算次数 越大波动越小 但延迟越大
	float downhillUperLimit = -uphillUperLimit,flatLowLimit = -flatUperLimit; // 上下坡上下限

	// 计算平均值减少波动
	if (readAngleCount < countTiem)
	{
		AngleSum += Angle_Y_Final;
		readAngleCount++;
	}else{
		AngleYAver = AngleSum / countTiem;
		readAngleCount = 0;
		AngleSum = 0;
	}

	// 大于上限为上坡状态
	if (AngleYAver > uphillUperLimit) carStatus = 1;
	if (AngleYAver < downhillUperLimit) carStatus = -1;
		
	if ((carStatus == 1 || carStatus == -1) && AngleYAver < flatUperLimit &&  AngleYAver > flatLowLimit){
		// 由上下坡状态变为水平状态
		carStatus = 0;
	}

	#if(isWork_OLED == 1)
		OLED_ShowString(2,13,carStatus == 1?"Up  ":carStatus == -1?"Down":"Flat");
	#endif

	return carStatus;
}

// 十字计数函数
void readCross(void){
	while (1)
	{ // 不断等待条件成立
		track(mainSpeed,mainSpeed);
		if (irState[0] && !irState[1] && !irState[2] && irState[3])
		{
			// 结束十字黑线识别
			// lastDetectionBlack = 0; // 清除标志位
			count++; // 十字计数加一
			#if(isWork_OLED == 1)
				OLED_ShowNum(4,1,count,1);
			#endif
			break;
		}
	}
}

// 通过管道
/*
void goPipeline(void){
	// 通过openmv上的红外传感器检测管道
	if (Serial_GetRxFlag() == 1)
	{
		RxData = Serial_GetRxData();
		if (RxData == 'P')
		{

		}
	}
}
*/

// 上波
void goBridge(void){
	short int carStatusRes = 0;
	short int isUperhill = 0;
	short int isDownhill = 0;
	while (1)
	{
		// 上下坡状态检测
		carStatusRes = checkCarStatus(10.0,4.0);
		// sprintf((char *)tmp_buf,"%.2f ",AngleYAver);
		// OLED_ShowString(2,5,tmp_buf); // 显示陀螺仪平均数值
		// track(mainSpeed,mainSpeed);
		if (!goBridgeFlage){
			// 没上桥 在十字与桥之间短暂循迹
			track(mainSpeed,mainSpeed); 
		}else{
			track(mainSpeed,mainSpeed);
			if (carStatusRes == 1){
				// 上波
				track(mainSpeed + 10,mainSpeed + 8);
				isUperhill = 1;
			}else if(carStatusRes == -1)
			{
				// 下坡
				track(mainSpeed -30,mainSpeed -27);
				isDownhill = 1;
			}else{
				// 处于水平位置
				if (isUperhill) {
					track(mainSpeed -25,mainSpeed-25); // 处于坡顶水平位置
				}else{
					track(mainSpeed -30,mainSpeed -30);
				}
			}
			// if ((irState[0] && !irState[1] && !irState[2] && irState[3]))
			// {
			// 	// 识别到已完成下坡 回到主循环循迹
			// 	break;
			// }
			if (carStatusRes == 0 && isDownhill)
			{
				// 识别到已完成下坡 回到主循环循迹
				int flatcount = 0;
				while (1)
				{
					track(mainSpeed -30,mainSpeed -27);
					if ((irState[0] && !irState[1] && !irState[2] && irState[3]))
					{
						flatcount++;
						if (flatcount >= 100)
						{
							break;
						}
						
					}
				}
				break;
			}
		}
		if (!irState[0] && !irState[1] && !irState[2] && !irState[3])
		{// 再次全黑代表开始上桥
			goBridgeFlage = 1; // 上桥标志位置1
		}
	}
}


// 过阶梯
void goStep(void){
	short int carStatusRes = 0;
	short int uperStage1 = 0,uperStage2 = 0,
			flatStep1 = 0,flatStep2 = 0,
			downStage1 = 0,downStage2 = 0,
			flatStep3 = 0,flatStep4 = 0;
	Servo1_SetAngle(20); // 舵机回到初始位置
	while (1)
	{
		// 上下坡状态检测
		if (!flatStep2)
		{
			carStatusRes = checkCarStatus(17.0,4.5); // 小轮21 四大轮17
		}else{
			// 下阶梯
			carStatusRes = checkCarStatus(13.5,4.5); // 小轮21  两大轮13.5
		}
		track(mainSpeed - 20,mainSpeed - 24);
		if (!goStepFlage){
			// 没过阶梯 在十字与桥之间短暂循迹
			// track(mainSpeed + 5,mainSpeed + 5); // 在上阶梯之前加速
			track(mainSpeed - 20,mainSpeed - 24);
		}else{
			
			if (carStatusRes == 1 && !flatStep1)
			{
				// track(mainSpeed-10,mainSpeed -13);
				// 在上第一个台阶
				uperStage1 = 1;
				
			}

			if (uperStage1 && carStatusRes == 0 && !uperStage2)
			{
				// 在第一个阶梯水平处
				flatStep1 = 1;
			}

			if (carStatusRes == 1 && flatStep1)
			{
				// 在上第二个台阶
				uperStage2 = 1;
				
			}

			if (uperStage2 && carStatusRes == 0 )
			{
				// 在第二个阶梯水平处
				flatStep2 = 1;
			}

			if (flatStep2 && carStatusRes == -1)
			{
				// 下首个台阶
				downStage1 = 1;		
				// track(mainSpeed -65,mainSpeed -65);
			}

			if (downStage1 && carStatusRes == 0)
			{
				// 在下第二个阶梯水平处
				flatStep3 = 1;
			}

			if (flatStep3 && carStatusRes == -1 && !downStage2)
			{
				// 在下最后一个台阶
				downStage2 = 1;
			}

			if (downStage2 && carStatusRes == 0)
			{
				// 到达平地
				flatStep4 = 1;		
			}

			if (flatStep2)
			{
				// 下台阶
				track(mainSpeed -47,mainSpeed -45);
			}else{
				track(mainSpeed - 20,mainSpeed - 24);
			}
			
			
			// if ((irState[0] && !irState[1] && !irState[2] && irState[3]) && carStatusRes == 0 && flatStep4)
			// { // 1001
			// 	// 识别到已完成下阶梯 回到主循环循迹
			// 	break;
			// }

			if (carStatusRes == 0 && flatStep4)
			{
				// 识别到已完成下坡 回到主循环循迹
				int flatcount = 0;
				while (1)
				{
					track(mainSpeed -30,mainSpeed -27);
					if ((irState[0] && !irState[1] && !irState[2] && irState[3]))
					{
						flatcount++;
						if (flatcount >= 50)
						{
							break;
						}
						
					}
				}
				break;
			}

		}
		if (!irState[0] && !irState[1] && !irState[2] && !irState[3])
		{// 再次全黑代表开始阶梯
			goStepFlage = 1; // 上阶梯标志位置1
		}
	}
}

// 颜色卡检测
void checkColorCard(void){
	// 发送O让openmv开始工作
	Serial_SendByte(OpenMv_USART,'O');
	// 等待接收openmv回传结果
	while (1)
	{
		track(mainSpeed - 20,mainSpeed - 20);
		// if (irState[0] && irState[1] && irState[2] && irState[3]) readCross(); // 识别黑色十字
		// 是否开启循迹标志位
		int isGOTrack = 0;
		if (!isGOTrack) track(mainSpeed - 20,mainSpeed - 20);
		if (Serial_GetRxFlag() == 1)
		{
			// 接收到的数据（一个字节）
			RxData = Serial_GetRxData();
			#if(isWork_OLED == 1)
				OLED_ShowChar(3, 1, RxData);
			#endif
			// 完成第一个目标色卡识别，开启循迹
			if (RxData == 'G') isGOTrack = 1;
			if (RxData == 'S')
			{
				// 控制舵机旋转
				Motor_braking();
				Servo1_SetAngle(180);
				delay_ms(1600);
				Servo1_SetAngle(90);
				break;
			}
		}
	}
}

// 冲向终点
void goTerminus(void){
	while (1)
	{
		// 快速循迹，冲向终点
		track(mainSpeed + 15,mainSpeed + 15);
		// if (!irState[0] && !irState[1] && !irState[2] && !irState[3]) readCross(); // 识别黑色十字
		if (irState[0] && irState[1] && irState[2] && irState[3])
		{
			delay_ms(500);
			Motor_stop();
			Servo1_SetAngle(5);
			while (1){// 全程结束 不再循迹
				GPIO_SetBits(GPIOB, GPIO_Pin_12);
				GPIO_SetBits(GPIOB, GPIO_Pin_13);
				GPIO_SetBits(GPIOB, GPIO_Pin_14);
				delay_ms(100);
				GPIO_ResetBits(GPIOB, GPIO_Pin_12);
				GPIO_ResetBits(GPIOB, GPIO_Pin_13);
				GPIO_ResetBits(GPIOB, GPIO_Pin_14);
				delay_ms(100);
			} 
		}
	}
}

